#!/usr/bin/env python
# -*-coding:utf-8 -*-

__all__ = ['RobotController', 'getRobots', 'getMap', 'generateRandTask', 'getTimer', 'run']

import logging
from message import Message


class RobotController(object):
    STATE_IDLE            = 0
    STATE_TASK_ASSIGNED   = 1
    STATE_TASK_PROCESSING = 2 # going to destination node, specified by task
    STATE_PAUSED          = 3
    
    def __init__(self, _id, comm_channel, timer, map):
        """
        创建一个机器人控制器。
        :param _id: 被本控制器所控制的机器人的ID。
        :param comm_channel: 通信信道（CommunicationChannel对象）。本控制器通过该通信信道与机器人通信。
        :param timer: 定时器。
        """
        self.initLogger()
        # At this moment, we just assumes that the controller knows the ID, waypoint, and state of the real robot.
        self._id = _id
        self._comm_channel = comm_channel
        self._timer = timer
        self._map = map
        
        self._last_waypoint = None
        self._next_waypoint = None
        self._current_speed = 0.0
        self._remaining_time = float('inf') # 机器人在当前边上还要继续行进多长时间
        self._on_edge = False  # 机器人是否在边上（初始时默认在节点上，因此为False）
        
        self._state = RobotController.STATE_IDLE
        
        self._composite_task = None
        self._current_simple_task = None
        
        self._path = None
        self._waypoint_time = None
        self._speed = None
        
        self._pos_updated = False
        
    def initLogger(self):
        # create logger
        self._logger = logging.getLogger('MyTCS.RobotController')
    
    def getControlledRobotID(self):
        return self._id
        
    def getScheduledPath(self):
        return self._path
        
    def getLastWaypoint(self):
        return self._last_waypoint
        
    def getNextWaypoint(self):
        return self._next_waypoint
                
    def getCurrentWaypoint(self):
        # if we don't know the position of the robot, or the robot is not moving, then there is no 'next waypoint'
        if self._state != RobotController.STATE_IDLE and \
           self._state != RobotController.STATE_TASK_ASSIGNED:
            return None
        return self._last_waypoint
        
    def getCompositeTask(self):
        return self._composite_task
        
    def getCurrentSimpleTask(self):
        return self._current_simple_task
        
    def isOnEdge(self):
        return self._on_edge
    
    def pause(self):
        self._state = RobotController.STATE_PAUSED
        self._comm_channel.serverSendMessage(Message(self._timer.getTime(),
                                                     None, # we don't need to specify the receiver, since there is only one server.
                                                     self._id, 
                                                     Message.CMD_TCS_PAUSE, 
                                                     None, 
                                                     delay=0.0))
        #self._remaining_time = float('inf')
        #print('Time=', self._timer.getTime(), 'robot', self._id, 'paused')
                                                     
    def schedule(self, path, waypoint_time):
        """
        调度机器人。
        :param path: 调度机器人按照该路径移动。
        :param waypoint_time: 调度机器人按照该时间序列依次抵达路径上的节点。
        """
        self._logger.debug('Controller sending schedule message to robot {0}, path {1}, waypoint_time {2}'.format(self._id, path, waypoint_time))
        if self.isOnEdge():
            if waypoint_time[1]-waypoint_time[0] > 1e-6:
                self._current_speed = self._current_speed*self._remaining_time/(waypoint_time[1]-waypoint_time[0])
        else:
            if waypoint_time[1]-waypoint_time[0] > 1e-6:
                self._current_speed = self._map.getEdgeWeight((path[0], path[1]))/(waypoint_time[1]-waypoint_time[0])
        self._speed = [self._current_speed] + [self._map.getEdgeWeight((path[i], path[i+1]))/(waypoint_time[i+1]-waypoint_time[i]) for i in range(1, len(path)-1)]
        self._comm_channel.serverSendMessage(Message(self._timer.getTime(),
                                                     None, # we don't need to specify the receiver, since there is only one server.
                                                     self._id, 
                                                     Message.CMD_TCS_SCHEDULE, 
                                                     (path, self._speed), 
                                                     delay=0.0))
        self._path = path
        self._waypoint_time = waypoint_time
        self._state = RobotController.STATE_TASK_PROCESSING
        idx = self._path.index(self._last_waypoint)
        self._next_waypoint = self._path[idx+1]
        self._remaining_time = self._waypoint_time[idx+1]-self._timer.getTime()
        self._logger.debug('Controller sent schedule message to robot {0}, path {1}, speed {2}'.format(self._id, self._path, self._speed))
    
    def getState(self):
        return self._state
        
    def getMaxSpeed(self):
        return 1.0
    
    def assign(self, ctask):
        if self._state == RobotController.STATE_IDLE:
            self._composite_task = ctask
            self._current_simple_task = ctask.getNextUncompletedTask()
            self._current_simple_task.setArrivalTime(self._timer.getTime())
            self._state = RobotController.STATE_TASK_ASSIGNED
            
    def setCurrentSimpleTaskCompleted(self):
        self._composite_task.setFirstTaskCompleted()
        self._path = None
        self._waypoint_time = None
        self._next_waypoint = None
        self._remaining_time = float('inf')
        self._current_speed = 0.0
        if self._composite_task.isEmpty():
            self._state = RobotController.STATE_IDLE
            self._current_simple_task = None
        else:
            self._current_simple_task = self._composite_task.getNextUncompletedTask()
            self._current_simple_task.setArrivalTime(self._timer.getTime())
    
    def isPosUpdated(self):
        return self._pos_updated
            
    def clearPosUpdatedFlag(self):
        self._pos_updated = False
        
    def isNewTaskArrived(self):
        return (self._current_simple_task is not None) and (self._current_simple_task.isNotScheduled())
        
    def getRemainingTimeOnCurrentEdge(self):
        if self._remaining_time > 0:
            return self._remaining_time
        else:
            return 0.0
            
    def process(self, dt):
        if self._state == RobotController.STATE_TASK_PROCESSING and self._path is not None:
            #self._logger.debug('Time={0:.2f} scheduled path of robot {1} is {2}: B'.\
            #                   format(self._timer.getTime(), self._id, self._path))
            #self._logger.debug('Time={0:.2f} next waypoint robot {1} is {2}'.\
            #                   format(self._timer.getTime(), self._id, self._next_waypoint))
            idx = self._path.index(self._next_waypoint)
            #print('Next waypoint time', self._waypoint_time[idx])
            #print('Time', self._timer.getTime())
            self._remaining_time = self._waypoint_time[idx] - self._timer.getTime()
            #print('Robot', self._id, 'path', self._path, 'waypoint time', self._waypoint_time, 'last waypoint',\
            #      self._last_waypoint, 'next waypoint', self._next_waypoint, 'remaining time', self._remaining_time)
            if not self._on_edge:
                self._on_edge = True
            
        msg = self._comm_channel.serverFetchMessage(self._id)
        if msg:
            if msg.getCommand() == Message.CMD_ROBOT_POS_UPDATE:
                self._pos_updated = True
                self._logger.debug('Time={0:.2f} robot {1}\'s controller received position update ({2}-->{3}) message'.\
                                   format(self._timer.getTime(), self._id, self._last_waypoint, msg.getParam()))
                self._last_waypoint = msg.getParam()
                self._on_edge = False
                if self._state == RobotController.STATE_TASK_PROCESSING:
                    if self._last_waypoint == self._current_simple_task.getDst():
                        self.setCurrentSimpleTaskCompleted()
                    elif self._path is not None:
                        self._logger.debug('Time={0:.2f} scheduled path of robot {1} is {2}: A'.\
                                   format(self._timer.getTime(), self._id, self._path))
                        idx = self._path.index(self._last_waypoint)
                        self._next_waypoint = self._path[idx + 1]
                        self._remaining_time = self._waypoint_time[idx+1] - self._timer.getTime()
                        self._current_speed = self._speed[idx]
                else:
                    self._next_waypoint = None
                    self._remaining_time = float('inf')
                    self._current_speed = 0.0
            return True 
        return False # nothing is processed